#include "main.hpp"

float current_x = 0.0;
float current_y = 0.0;
float current_z = 0.0;
float current_time = 0.0;
float target_x = 0.0;
float target_y = 0.0;
float target_z = 0.0;
float target_time = 0.0;
float target_T = 0.0;
int8_t sign = 0.0;
uint8_t IsIdentify = 0x00;

//订阅方回调函数
class Handler 
{
    public:
        ~Handler() {}
        void handleMessage(const lcm::ReceiveBuffer* rbuf,
                const std::string& chan, 
                const target::target_t* msg)
        {
            current_x = msg->current_x;
            current_y = msg->current_y;
            current_z = msg->current_z;
            current_time = msg->current_time;
            target_x = msg->target_x;
            target_y = msg->target_y;
            target_z = msg->target_z;
            target_time = msg->target_time;
            target_T = msg->target_T;
        }
};
void* camera_thread(void* arg)
{
    uint8_t loop_1Hz  = 0;
    uint8_t loop_5Hz  = 0;
    uint8_t loop_10Hz = 0;
    uint8_t loop_20Hz = 0;
    uint8_t loop_50Hz = 0;
    uint8_t loop_100Hz = 0;
    uint64_t time_5ms = 0;

    usr_thread_t *temp = (usr_thread_t*)arg;
    uint8_t camera_rcv_buf[128];

    //创建LCM对象
    lcm::LCM lcm;
    Handler handlerObject;
    lcm.subscribe("YOLO", &Handler::handleMessage, &handlerObject);
    
    while(1)
    {
 
        if((micros()-time_5ms)>=5000){
            time_5ms = micros();
            loop_100Hz++;
            loop_20Hz++;
            loop_50Hz++;
            loop_10Hz++;
            loop_5Hz++;
            loop_1Hz++;	
        }
        if(loop_100Hz>=2){
            loop_100Hz = 0;

        }
        if(loop_50Hz>=4){
            loop_50Hz = 0;
            
        }
        
        if(loop_20Hz>=10){
            loop_20Hz = 0;
	    lcm.handleTimeout(5);
            LCM_send_sign_to_ros(0x02);
	    IsIdentifyState();
	    printf("current_x: %f , current_y: %f,current_z: %f, target_x: %f, target_y: %f,target_z: %f \n",current_x,current_y,current_z,target_x,target_y,target_z);
        }
        if(loop_10Hz>=20){     
            loop_10Hz = 0;
            
            
        }
        if(loop_5Hz>=40){
            loop_5Hz = 0;

        }
        
        if(loop_1Hz>=200){
            loop_1Hz = 0;

         std::cout << " 我是camera线程：" << std::endl;
  
        }
        
    }
    
}

//LCM发送观测信号给ROS,不在圈内发0，在圈内发2
void LCM_send_sign_to_ros(uint8_t sign){
    lcm::LCM lcm;
    if (!lcm.good())
	{
		cout<<"LCM error!!!"<<endl;
	}
    target::target_t my_data;
    my_data.sign = sign;
    lcm.publish("YOLO_sign", &my_data);
    //printf("sign : %d \n" , sign);
}
//识别状态判断
void IsIdentifyState(){
    if(current_x == 0 && current_y == 0 && current_z == 0){
        IsIdentify = 0x00;
    }
    else{
        IsIdentify = 0x01;
    }
}

